package recallin.navigation;

import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map.Entry;
import java.util.PriorityQueue;

import org.mapsforge.core.GeoPoint;

import android.widget.Toast;
/**
 * connects all the nodes through the ways to gethere and find the shortest path using Dijkstra
 * also find the navigation information for turnings
 * create triggers for the navigation mode
 * @author Khashayar , Muhanad
 *
 */


class Vertex implements Comparable<Vertex> {
	public final String name;
	public Edge[] adjacencies = new Edge[0];
	public double minDistance = Double.POSITIVE_INFINITY;
	public Vertex previous;
	public static String User_paht1;
	public static String User_path2;
	public long[] ways = new long[0];
	public Double latitude;
	public Double longitude;

	public Vertex(String argName, Double lat, Double lon) {
		name = argName;
		latitude = lat;
		longitude = lon;
	}

	public String toString() {
		String s = name + " : [";
		for (long l : ways) {
			s += l + " , "; 
		}
		return s + "]";
	}

	public int compareTo(Vertex other) {
		return Double.compare(minDistance, other.minDistance);
	}

}

class Edge {
	public final Vertex target;
	public final double weight;

	public Edge(Vertex argTarget, double argWeight) {

		target = argTarget;
		weight = argWeight;
	}
}

public class FindPath{

	private HashMap<Long, Way> ways;
	private HashMap<Long, Node> nodes; 
	private DecimalFormat df = new DecimalFormat("###.###");
	private Double boxTop;
	private Double boxLeft;

	

	public FindPath(HashMap<Long, Way> ways, HashMap<Long, Node> nodes) {
		this.ways = ways;
		this.nodes = nodes;
	}

	public static void computePaths(Vertex source) {
		source.minDistance = 0.;
		PriorityQueue<Vertex> vertexQueue = new PriorityQueue<Vertex>();
		vertexQueue.add(source);

		while (!vertexQueue.isEmpty()) {
			Vertex u = vertexQueue.poll();

			// Visit each edge exiting u
			for (Edge e : u.adjacencies) {
				Vertex v = e.target;
				double weight = e.weight;
				double distanceThroughU = u.minDistance + weight;
				if (distanceThroughU < v.minDistance) {
					vertexQueue.remove(v);

					v.minDistance = distanceThroughU;
					v.previous = u;
					vertexQueue.add(v);

				}

			}
		}
	}

	public static List<Vertex> getShortestPathTo(Vertex target) {
		List<Vertex> path = new ArrayList<Vertex>();
		for (Vertex vertex = target; vertex != null; vertex = vertex.previous)
			path.add(vertex);
		Collections.reverse(path);
		return path;

	}

	@SuppressWarnings({ "unused", "rawtypes", "unchecked" })
	public PathWay getWay(Long startNode , Long endNode) {


		HashMap<Long, Vertex> hashMap = new HashMap<Long, Vertex>();

		// Vertex[] allVertices = new Vertex[nodes.size()];
		Iterator iteratorNode = nodes.entrySet().iterator();
		int countNode = 0;
		while (iteratorNode.hasNext()) {
			Entry<Long, Node> nodeSet = (Entry<Long, Node>) iteratorNode.next();
			Node node = nodeSet.getValue();
			// allVertices[countNode] = new
			// Vertex(nodeSet.getKey().toString(),node.getLatitude(),
			// node.getLongitude());
			hashMap.put(nodeSet.getKey(), new Vertex(nodeSet.getKey()
					.toString(), node.getLatitude(), node.getLongitude()));
			countNode++;
		}

		boolean flag;
		int flagPosition = 0;

		Long lastNodeId;
		Long curruntNodeId;
		Long beforeNodeId;
		Long afterNodeId;
		double beforeDistance;
		double afterDistance;

		int curruntIndex = -1;
		int beforeIndex = -1;
		int afterIndex = -1;

		Iterator iterator = ways.entrySet().iterator();
		while (iterator.hasNext()) {
			Entry<Long, Way> waySet = (Entry<Long, Way>) iterator.next();
			Way way = waySet.getValue();
			Long thisWayId = waySet.getKey();

			boolean oneWay = false;

			if (way.getTags().containsKey("oneway")) {
				oneWay = true;
			}

			if (oneWay) {
				for (int i = 0; i < way.getNodes().size() - 1; i++) {
					flag = false;

					curruntNodeId = way.getNodes().get(i);
					afterNodeId = way.getNodes().get(i + 1);
					afterDistance = (CalculateDistance(
							nodes.get(curruntNodeId), nodes.get(afterNodeId)));

					/**
					 * New Implementation
					 */
					Vertex currentVertex = hashMap.get(curruntNodeId);
					Vertex afterVertex = hashMap.get(afterNodeId);

					Edge[] tempCurrentVertexEdges = currentVertex.adjacencies;
					currentVertex.adjacencies = new Edge[tempCurrentVertexEdges.length + 1];
					for (int k = 0; k < tempCurrentVertexEdges.length; k++) {
						currentVertex.adjacencies[k] = tempCurrentVertexEdges[k];
					}
					currentVertex.adjacencies[tempCurrentVertexEdges.length] = new Edge(
							afterVertex, afterDistance);

					long[] tempCurrentVertexWays = currentVertex.ways;
					currentVertex.ways = new long[tempCurrentVertexWays.length + 1];
					for (int k = 0; k < tempCurrentVertexWays.length; k++) {
						currentVertex.ways[k] = tempCurrentVertexWays[k];
					}
					currentVertex.ways[tempCurrentVertexWays.length] = thisWayId;

					/**
					 * End Of This New Thing
					 */

				}
				if(!way.getTags().containsKey("round")){
					lastNodeId = way.getNodes().get(way.getNodes().size() - 1);
					Vertex lastVertex = hashMap.get(lastNodeId);
					long[] tempLastVertexWays = lastVertex.ways;
					lastVertex.ways = new long[tempLastVertexWays.length + 1];
					for (int k = 0; k < tempLastVertexWays.length; k++) {
						lastVertex.ways[k] = tempLastVertexWays[k];
					}
					lastVertex.ways[tempLastVertexWays.length] = thisWayId;
				}

			} else {
				lastNodeId = way.getNodes().get(way.getNodes().size() - 1);
				beforeNodeId = way.getNodes().get(way.getNodes().size() - 2);
				beforeDistance = (CalculateDistance(nodes.get(lastNodeId),
						nodes.get(beforeNodeId)));

				/**
				 * New Implementation
				 */
				Vertex lastVertex = hashMap.get(lastNodeId);
				Vertex previousVertex = hashMap.get(beforeNodeId);

				Edge[] tempLastVertexEdges = lastVertex.adjacencies;
				lastVertex.adjacencies = new Edge[tempLastVertexEdges.length + 1];
				for (int k = 0; k < tempLastVertexEdges.length; k++) {
					lastVertex.adjacencies[k] = tempLastVertexEdges[k];
				}
				lastVertex.adjacencies[tempLastVertexEdges.length] = new Edge(
						previousVertex, beforeDistance);

				long[] tempLastVertexWays = lastVertex.ways;
				lastVertex.ways = new long[tempLastVertexWays.length + 1];
				for (int k = 0; k < tempLastVertexWays.length; k++) {
					lastVertex.ways[k] = tempLastVertexWays[k];
				}
				lastVertex.ways[tempLastVertexWays.length] = thisWayId;

				/**
				 * End Of This New Thing
				 */

				curruntNodeId = way.getNodes().get(0);
				afterNodeId = way.getNodes().get(1);
				afterDistance = (CalculateDistance(nodes.get(curruntNodeId),
						nodes.get(afterNodeId)));

				Vertex firstVertex = hashMap.get(curruntNodeId);
				Vertex secondVertex = hashMap.get(afterNodeId);

				Edge[] tempFirstVertexEdges = firstVertex.adjacencies;
				firstVertex.adjacencies = new Edge[tempFirstVertexEdges.length + 1];
				for (int k = 0; k < tempFirstVertexEdges.length; k++) {
					firstVertex.adjacencies[k] = tempFirstVertexEdges[k];
				}
				firstVertex.adjacencies[tempFirstVertexEdges.length] = new Edge(
						secondVertex, beforeDistance);

				long[] tempFirstVertexWays = firstVertex.ways;
				firstVertex.ways = new long[tempFirstVertexWays.length + 1];
				for (int k = 0; k < tempFirstVertexWays.length; k++) {
					firstVertex.ways[k] = tempFirstVertexWays[k];
				}
				firstVertex.ways[tempFirstVertexWays.length] = thisWayId;

				/**
				 * End Of This New Thing
				 */

				for (int i = 1; i < way.getNodes().size() - 1; i++) {
					flag = false;

					curruntNodeId = way.getNodes().get(i);
					beforeNodeId = way.getNodes().get(i - 1);
					afterNodeId = way.getNodes().get(i + 1);
					beforeDistance = (CalculateDistance(
							nodes.get(curruntNodeId), nodes.get(beforeNodeId)));
					afterDistance = (CalculateDistance(
							nodes.get(curruntNodeId), nodes.get(afterNodeId)));

					/**
					 * New Implementation
					 */
					Vertex currentVertex = hashMap.get(curruntNodeId);
					Vertex afterVertex = hashMap.get(afterNodeId);
					Vertex beforeVertex = hashMap.get(beforeNodeId);

					Edge[] tempCurrentVertexEdges = currentVertex.adjacencies;
					currentVertex.adjacencies = new Edge[tempCurrentVertexEdges.length + 2];
					for (int k = 0; k < tempCurrentVertexEdges.length; k++) {
						currentVertex.adjacencies[k] = tempCurrentVertexEdges[k];
					}
					currentVertex.adjacencies[tempCurrentVertexEdges.length] = new Edge(
							afterVertex, afterDistance);
					currentVertex.adjacencies[tempCurrentVertexEdges.length + 1] = new Edge(
							beforeVertex, beforeDistance);

					long[] tempCurrentVertexWays = currentVertex.ways;
					currentVertex.ways = new long[tempCurrentVertexWays.length + 1];
					for (int k = 0; k < tempCurrentVertexWays.length; k++) {
						currentVertex.ways[k] = tempCurrentVertexWays[k];
					}
					currentVertex.ways[tempCurrentVertexWays.length] = thisWayId;

					/**
					 * End Of This New Thing
					 */

				}

			}
		}
		int theOneWeWantStart = 0;
		int theOneWeWantEnd = 0;

		computePaths(hashMap.get(startNode));
		List<Vertex> path = getShortestPathTo(hashMap.get(endNode));


		GeoPoint[][] geoPoints = new GeoPoint[1][path.size()];
			
		for (int i = 0; i < path.size(); i++) {
			Vertex vertex = path.get(i);
			geoPoints[0][i] = new GeoPoint(vertex.latitude, vertex.longitude);
		}
		
		Long currentWay = 0l;
		try {
			for (long id1 : path.get(0).ways) {
				for (long id2 : path.get(1).ways) {
					if(id1 == id2)
						currentWay = Long.valueOf(id1);
				}
			}
		} catch (IndexOutOfBoundsException e) {
			geoPoints = new GeoPoint[1][2];
			geoPoints[0][0] = new GeoPoint(nodes.get(startNode).getLatitude(), nodes.get(startNode).getLongitude());
			geoPoints[0][1] = new GeoPoint(nodes.get(endNode).getLatitude(), nodes.get(endNode).getLongitude());
		}
		PathWay pathWay = new PathWay();
		System.out.println("CUR : " + currentWay);
		
		int exit = 1;
		
		pathWay.setGeopoint(geoPoints);
		
		
		ArrayList<MultiNode> multiNodes = new ArrayList<MultiNode>();
		for (int i = 1; i < path.size() -1 ; i++) {
			int exitRight = 0;
			int exitLeft = 0;
			for (MultiNode m : multiNodes) {
				if(m.getLeft_or_right() == -1)
					exitLeft++;
				if(m.getLeft_or_right() == 1)
					exitRight++;
				if(m.getLeft_or_right() == 0){
					exitLeft++;
					exitRight++;
				}
			}
			
			if(exitLeft > 2){
				int indextoremove = -1;
				for (int j = 0; j < multiNodes.size(); j++) {
					if(multiNodes.get(j).getLeft_or_right() == -1 || multiNodes.get(j).getLeft_or_right() == 0)
						if(indextoremove == -1)
							indextoremove = j;
				}
				for (int j = 0; j < indextoremove; j++) {
					multiNodes.remove(0);
				}
			}

			if(exitRight > 2){
				int indextoremove = -1;
				for (int j = 0; j < multiNodes.size(); j++) {
					if(multiNodes.get(j).getLeft_or_right() == 1 || multiNodes.get(j).getLeft_or_right() == 0)
						if(indextoremove == -1)
							indextoremove = j;
				}
				for (int j = 0; j < indextoremove; j++) {
					multiNodes.remove(0);
				}
			}
			
			Vertex v = path.get(i);
			ArrayList<Long> availableWays = isAnyOtherWay(v, currentWay);
			if(v.ways.length == 1 && currentWay.equals(Long.valueOf(v.ways[0]))){
			}else{
				Long nextWay = 0l;
				for (long id1 : path.get(i).ways) {
					for (long id2 : path.get(i+1).ways) {
						if(id1 == id2)
							nextWay = Long.valueOf(id1);
					}
				}
				if(!nextWay.equals(currentWay)){

					Node curWayLastNode = nodes.get(this.ways.get(Long.valueOf(currentWay)).getNodes().get(this.ways.get(Long.valueOf(currentWay)).getNodes().size()-1));
					Node curWayFirstNode = nodes.get(this.ways.get(Long.valueOf(currentWay)).getNodes().get(0));

					Node nextWayLastNode = nodes.get(this.ways.get(Long.valueOf(nextWay)).getNodes().get(this.ways.get(Long.valueOf(nextWay)).getNodes().size()-1));
					Node nextWayFirstNode = nodes.get(this.ways.get(Long.valueOf(nextWay)).getNodes().get(0));
					
					if( ((curWayLastNode.getLatitude().equals(v.latitude) && curWayLastNode.getLongitude().equals(v.longitude)) || 
							(curWayFirstNode.getLatitude().equals(v.latitude) && curWayFirstNode.getLongitude().equals(v.longitude)) ) 
							&&
							((nextWayLastNode.getLatitude().equals(v.latitude) && nextWayLastNode.getLongitude().equals(v.longitude)) || 
							(nextWayFirstNode.getLatitude().equals(v.latitude) && nextWayFirstNode.getLongitude().equals(v.longitude)) )
							&& availableWays.size()==1){
						//Do Nothing
					}else{
						ArrayList<Double> line1 = new ArrayList<Double>();
						ArrayList<Double> line2 = new ArrayList<Double>();
						
						line1.add(path.get(i-1).latitude);
						line1.add(path.get(i).latitude);
						line1.add(path.get(i-1).longitude);
						line1.add(path.get(i).longitude);
						
						line2.add(path.get(i).latitude);
						line2.add(path.get(i+1).latitude);
						line2.add(path.get(i).longitude);
						line2.add(path.get(i+1).longitude);
						
						if((angleBetween2Lines(line1, line2) > 10 && angleBetween2Lines(line1, line2) <180) ||
								(angleBetween2Lines(line1, line2) <-180 && angleBetween2Lines(line1, line2) >-350)){
							multiNodes.add(new MultiNode(v ,Long.valueOf(v.name), 1));
							int turn = 0;
							for (MultiNode multi : multiNodes) {
								if(multi.getLeft_or_right() == 1 || multi.getLeft_or_right() == 0)
									turn++;
							}
							String turnString = "";
							switch (turn) {
							case 1:
								turnString = "first";
								break;
							case 2:
								turnString = "second";
								break;
							case 3:
								turnString = "third";
								break;
							}
							String message = "take the " + turnString +  " exit on the right";
							GeoPoint turnPoint = new GeoPoint(nodes.get(multiNodes.get(0).getId()).getLatitude(), nodes.get(multiNodes.get(0).getId()).getLongitude());
							Double lenght;
							if(pathWay.getTrigger().size() == 0){
								lenght = multiNodes.get(0).getVertex().minDistance;
							}else{
								lenght = multiNodes.get(0).getVertex().minDistance - pathWay.getTrigger().get(pathWay.getTrigger().size()-1).getVertex().minDistance;
							}
							Double distance = lenght / 2;
							
							pathWay.getTrigger().add(new Trigger(multiNodes.get(0).getVertex(), turnPoint, message, lenght, distance));
							
						}else{
							multiNodes.add(new MultiNode(v , Long.valueOf(v.name), -1));
							int turn = 0;
							for (MultiNode multi : multiNodes) {
								if(multi.getLeft_or_right() == -1 || multi.getLeft_or_right() == 0)
									turn++;
							}
							String turnString = "";
							switch (turn) {
							case 1:
								turnString = "first";
								break;
							case 2:
								turnString = "second";
								break;
							case 3:
								turnString = "third";
								break;
							}
							String message = "take the " + turnString +  " exit on the left";
							GeoPoint turnPoint = new GeoPoint(nodes.get(multiNodes.get(0).getId()).getLatitude(), nodes.get(multiNodes.get(0).getId()).getLongitude());
							Double lenght;
							if(pathWay.getTrigger().size() == 0){
								lenght = multiNodes.get(0).getVertex().minDistance;
							}else{
								lenght = multiNodes.get(0).getVertex().minDistance - pathWay.getTrigger().get(pathWay.getTrigger().size()-1).getVertex().minDistance;
							}
							Double distance = lenght / 2;
							
							pathWay.getTrigger().add(new Trigger(multiNodes.get(0).getVertex(), turnPoint, message, lenght, distance));
							
						}
						multiNodes.clear();
					}

					
				}else{
					ArrayList<Double> line1 = new ArrayList<Double>();
					ArrayList<Double> line2 = new ArrayList<Double>();

					line1.add(path.get(i-1).latitude);
					line1.add(path.get(i).latitude);
					line1.add(path.get(i-1).longitude);
					line1.add(path.get(i).longitude);
					
					for (Long id : availableWays) {
						Way way = ways.get(id);
						int size = way.getNodes().size();
						if(nodes.get(way.getNodes().get(0)).getLatitude().equals(v.latitude) &&
								nodes.get(way.getNodes().get(0)).getLongitude().equals(v.longitude)){
							
							line2.add(path.get(i).latitude);
							line2.add(nodes.get(way.getNodes().get(1)).getLatitude());
							line2.add(path.get(i).longitude);
							line2.add(nodes.get(way.getNodes().get(1)).getLongitude());

							if( (angleBetween2Lines(line1, line2) > 10 && angleBetween2Lines(line1, line2) <180) ||
									(angleBetween2Lines(line1, line2) <-180 && angleBetween2Lines(line1, line2) >-350)){
								multiNodes.add(new MultiNode(v, Long.valueOf(v.name), 1));
							}else{
								multiNodes.add(new MultiNode(v, Long.valueOf(v.name), -1));
							}

						}else if(nodes.get(way.getNodes().get(size-1)).getLatitude().equals(v.latitude) &&
									nodes.get(way.getNodes().get(size-1)).getLongitude().equals(v.longitude)){
								
								line2.add(path.get(i).latitude);
								line2.add(nodes.get(way.getNodes().get(size-2)).getLatitude());
								line2.add(path.get(i).longitude);
								line2.add(nodes.get(way.getNodes().get(size-2)).getLongitude());

								if((angleBetween2Lines(line1, line2) > 10 && angleBetween2Lines(line1, line2) <180) ||
										(angleBetween2Lines(line1, line2) <-180 && angleBetween2Lines(line1, line2) >-350)){
									multiNodes.add(new MultiNode(v, Long.valueOf(v.name), 1));
								}else{
									multiNodes.add(new MultiNode(v, Long.valueOf(v.name), -1));
								}
							
						}else{
								if(way.getTags().containsKey("oneway")){
									//TODO for round about
									int index = way.getNodes().indexOf(Long.valueOf(v.name));
									line2.add(path.get(i).latitude);
									line2.add(nodes.get(way.getNodes().get(index+1)).getLatitude());
									line2.add(path.get(i).longitude);
									line2.add(nodes.get(way.getNodes().get(index+1)).getLongitude());
									
									if((angleBetween2Lines(line1, line2) > 10 && angleBetween2Lines(line1, line2) <180) ||
											(angleBetween2Lines(line1, line2) <-180 && angleBetween2Lines(line1, line2) >-350)){
										multiNodes.add(new MultiNode(v, Long.valueOf(v.name), 1));
									}else{
										multiNodes.add(new MultiNode(v, Long.valueOf(v.name), -1));
									}
								
								}else{
									multiNodes.add(new MultiNode(v, Long.valueOf(v.name), 0));
								}
							}
					}
					
					
				}
				currentWay = nextWay;
				
				
			}
			
		}
		return pathWay;
	}

	private double CalculateDistance(Node a, Node b) {
		  double R = 6371; // km
		  double dLat = Math.toRadians((b.getLatitude() - a.getLatitude()));
		  double dLon = Math.toRadians(b.getLongitude() - a.getLongitude());
		  double lat1 = Math.toRadians(a.getLatitude());
		  double lat2 = Math.toRadians(b.getLatitude());

		  double a1 = Math.sin(dLat / 2) * Math.sin(dLat / 2)
		    + Math.sin(dLon / 2) * Math.sin(dLon / 2) * Math.cos(lat1)
		    * Math.cos(lat2);
		  double c = 2 * Math.atan2(Math.sqrt(a1), Math.sqrt(1 - a1));
		  double d = R * c;
		  return (d * 1000);
	}
	
	private double angleBetween2Lines(ArrayList<Double> line1, ArrayList<Double> line2){
        double angle1 = Math.atan2(line1.get(0) - line1.get(1),
                                   line1.get(2) - line1.get(3));
        double angle2 = Math.atan2(line2.get(0) - line2.get(1),
                                   line2.get(2) - line2.get(3));
        return Math.toDegrees(angle1-angle2);
    }
	private ArrayList<Long> isAnyOtherWay(Vertex v , long currentWay) {
		
		ArrayList<Long> availableWays = new ArrayList<Long>();
		
		int availabe = 0;
		
		for (long id : v.ways) {
			if(id != currentWay){

				if(ways.get(Long.valueOf(id)).getTags().containsKey("oneway")){
					if(this.ways.get(Long.valueOf(id)).getTags().containsKey("round")){
						availableWays.add(Long.valueOf(id));
						availabe++;
					}else{
						Node nextWayLastNode = nodes.get(this.ways.get(Long.valueOf(id)).getNodes().get(this.ways.get(Long.valueOf(id)).getNodes().size()-1));
						if(nextWayLastNode.getLatitude().equals(v.latitude) && nextWayLastNode.getLongitude().equals(v.longitude)){
							//Do Nothing
						}else{
							availableWays.add(Long.valueOf(id));
							availabe++;
						}
					}
				}else{
					availableWays.add(Long.valueOf(id));
					availabe++;
				}
				
			}
		}
		
		return availableWays;
		
	}
}